package bluetooth;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;



import util.BarCode;
import util.SensorColour;

import algorithms.BarCodeScanner;
import algorithms.LineFinder;
import algorithms.PlayTune;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.localization.PoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;



public class RemoteControl implements IndigoProtocol {
	
	public int BLACK_BROWN = 387;
	public int BROWN_WHITE = 484;
	
	public int DEFAULT_ACCELERATION = 14;
	
	public static final double SENSOR_TO_WHEEL_DISTANCE = 2.1 + Math.PI;
	
	private static final int SECTOR_SIZE = 40;
	
	private DataOutputStream dataOut;
	private DataInputStream dataIn;
	
	private DifferentialPilot pilot;
	private OdometryPoseProvider poseProvider;
	private NXTRegulatedMotor sonarMotor;
	private LightSensor light;
	private UltrasonicSensor sonar;
	private TouchSensor touchLeft;
	private TouchSensor touchRight;
	
	/*
	 * Omdat, ik citeer, Michiel niet graag nadenkt:
	 * Wanneer de robot te weinig draait, moet dit getalletje
	 * hoger gezet worden (en vice versa)
	 */
	private double trackWidth = 17.4;
	private double wheelDiameter = 5.55;
	
	public static void main(String param[])
	{
		RemoteControl control = new RemoteControl();
		//control.test();
		control.start();
		
	}
	
	public void test()
	{
		List<Integer> samples = new ArrayList<Integer>();
		sonarMotor.rotateTo(90);
		pilot.setAcceleration(13);
		samples.add(sonar.getDistance());
		for(int i = 0; i < 2; i++)
		{
			pilot.travel(1);
			samples.add(sonar.getDistance());
		}
		for(int i = 0; i < 2; i++)
		{
			pilot.travel(-1);
			samples.add(sonar.getDistance());
		}

	   	// Sort
		 for (int i=0; i<samples.size()-1; i++) {
		        for (int j=i+1; j<samples.size(); j++) {
		            if (samples.get(i) > samples.get(j)) {
		                //... Exchange elements
		                int temp = samples.get(i);
		                samples.set(i, samples.get(j));
		                samples.set(j, temp);
		            }
		        }
		    }
		 String str = "";
		 for(int val : samples)
			 str += val + "/";
		 System.out.println(str);
		 try {
			Thread.sleep(100000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	

	public RemoteControl() 
	{
		pilot 		= new DifferentialPilot(wheelDiameter, trackWidth, Motor.A, Motor.C);
		poseProvider= new OdometryPoseProvider(pilot);
		sonarMotor 	= Motor.B;
		light 		= new LightSensor(SensorPort.S1);
		sonar 		= new UltrasonicSensor(SensorPort.S4);
		touchLeft 	= new TouchSensor(SensorPort.S3);
		touchRight 	= new TouchSensor(SensorPort.S2);
		poseProvider.setPose(new Pose(20, 20, 0));
		pilot.setAcceleration(20);
	}
	
	private void start()
	{
		connect();
		while(true)
			readAndExecute();
	}
	
	private void connect()
	{
		BTConnection connection;
		System.out.println("Waiting for pc to connect ...");
		connection = Bluetooth.waitForConnection();
		System.out.println("Connection established!");
		Sound.beepSequenceUp();
		pilot.travel(0.1);
		dataIn  = connection.openDataInputStream();
		dataOut = connection.openDataOutputStream(); 
	}
	
	private synchronized void readAndExecute()
    {
         try {
			int OPCode 				= dataIn.readInt();
	        double param1 			= dataIn.readDouble();
	        double param2 			= dataIn.readDouble();
	        boolean immediateReturn = dataIn.readBoolean();
	        executeCommand(OPCode, param1, param2, immediateReturn);
         } catch (IOException | InterruptedException e) {
 			System.out.println("Failed to read command from pc stream.");
 			Sound.buzz();
 			try {
				Thread.sleep(1000);
			} catch (InterruptedException e1) {
			}
 		}
    }
	
	public void immediateStop()
	{
		pilot.setAcceleration((int) (4*pilot.getMaxTravelSpeed()));
		pilot.stop();
		pilot.setAcceleration(DEFAULT_ACCELERATION);
	}
	
	private void sendSucces()
	{
		try {
			dataOut.writeBoolean(true);
			dataOut.flush();
		} catch (IOException e) {
			System.out.println("Failed to send reply, trying again");
			sendSucces();
		}
	}
	
	private void executeCommand(int OPCode, double param1, double param2, boolean immediateReturn) throws InterruptedException
	{
//		System.out.println("CMD: " + OPCode);
		if(OPCode == FORWARD)
		{
			pilot.forward();
		} 
		else if(OPCode == BACKWARD)
		{
			pilot.backward();
		}
		else if(OPCode == STOP)
		{
			pilot.stop();
		}
		else if(OPCode == TRAVEL)
		{
			pilot.travel(param1, immediateReturn);
			if(!immediateReturn)
				sendSucces();
		}
		else if(OPCode == TRAVELSAFELY)
		{
			pilot.travel(param1, true);
			
			while(pilot.isMoving()) {
				if ( getLightColour().equals(SensorColour.BLACK) ) {
					immediateStop();
					try {
						dataOut.writeInt(1);
						dataOut.flush();
					} catch (IOException e) {
						System.out.println("Failed to send light warning");
					}	
					return;
				}
				if ( touchLeft.isPressed() || touchRight.isPressed() ) {
					immediateStop();
					try {
						dataOut.writeInt(2);
						dataOut.flush();
					} catch (IOException e) {
						System.out.println("Failed to send touch warning");
					}
					return;
				}
				if ( sonar.getRange() < SECTOR_SIZE / 4) 
				{
					immediateStop();
					try {
						dataOut.writeInt(3);
						dataOut.flush();
					} catch (IOException e) {
						System.out.println("Failed to send sonar data");
					}
					return;
				}
			}
			
			try {
				dataOut.writeInt(0);
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to success data");
			}
		}
		else if(OPCode == ROTATELEFT)
		{
			pilot.rotateLeft();
		}
		else if(OPCode == ROTATERIGHT)
		{
			pilot.rotateRight();
		}
		else if(OPCode == ROTATE)
		{
			pilot.rotate(param1, immediateReturn);
			if(!immediateReturn)
				sendSucces();
		}
		else if(OPCode == SETROTATESPEED)
		{
			pilot.setRotateSpeed(param1);
		}
		else if(OPCode == SETTRAVELSPEED)
		{
			pilot.setTravelSpeed(param1);
		}
		else if(OPCode == SETACCELERATION)
		{
			pilot.setAcceleration((int) param1);
		}
		else if(OPCode == ROTATE_SONAR)
		{
			sonarMotor.rotateTo((int) param1);
			sendSucces();
		}
		else if(OPCode == GETLIGHTVALUE)
		{
			int lightValue = light.getNormalizedLightValue();
			try {
				dataOut.writeInt(lightValue);
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send light value data");
			}
		}
		else if(OPCode == GETSONARVALUE)
		{
			float sonarValue = sonar.getRange();
			try {
				dataOut.writeFloat(sonarValue);
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send sonar distance data");
			}
		}
		else if(OPCode == GETTOUCHLEFT)
		{
			boolean touchValue = touchLeft.isPressed();
			try {
				dataOut.writeBoolean(touchValue);
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send touch left data");
			}
		}
		else if(OPCode == GETTOUCHRIGHT)
		{
			boolean touchValue = touchRight.isPressed();
			try {
				dataOut.writeBoolean(touchValue);
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send touch right data");
			}
		}
		else if(OPCode == ISMOVING)
		{
			try {
				dataOut.writeBoolean(pilot.isMoving());
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send isMoving data");
			}
		}
		else if(OPCode == GETSONARTACHO)
		{
			try {
				dataOut.writeInt(sonarMotor.getTachoCount());
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send sonar tacho data");
			}
		}
		else if(OPCode == GETMAXROTATESPEED)
		{
			try {
				dataOut.writeFloat(pilot.getMaxRotateSpeed());
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send max rotate speed data");
			}
		}
		else if(OPCode == GETMAXTRAVELSPEED)
		{
			try {
				dataOut.writeDouble(pilot.getMaxTravelSpeed());
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send max travel speed data");
			}
		}
		else if(OPCode == GETMOVEMENT)
		{
			Move move = pilot.getMovement();			
			try {
				move.dumpObject(dataOut);
			} catch (Exception e) {
				System.out.println("Failed sending movement");
			}
		}
		else if(OPCode == GETPOSE)
		{
			Pose pose = poseProvider.getPose();
			try {
				pose.dumpObject(dataOut);
			} catch (Exception e) {
				System.out.println("Failed sending pose");
			}
		}
		else if(OPCode == SETPOSE)
		{
			Pose pose = new Pose();
			try {
				pose.loadObject(dataIn);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			poseProvider.setPose(pose);
		}
		else if(OPCode == FINDWHITELINE)
		{
			LineFinder finder = new LineFinder(this);
			finder.run(SensorColour.WHITE);
			sendSucces();
		}
		else if(OPCode == SCANBARCODE)
		{
			BarCodeScanner scanner = new BarCodeScanner(this);
			try {
				dataOut.writeInt(scanner.scan());
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed sending barcode data");
			}
		}
		else if(OPCode == SONARSCAN)
		{
			int direction = (int) param1;
			int result = sonarScan(direction);
			
			try {
				dataOut.writeInt(result);
				dataOut.flush();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		else if(OPCode == GOTOMIDDLE)
		{
			LineFinder finder = new LineFinder(this);
			finder.goToMiddle();
			sendSucces();
		}
		else if(OPCode == SCANSAFE)
		{
			float sonarValue = sonar.getRange();
			if(sonarValue>20&&sonarValue<40)
				sonarValue=sonarScan();
			try {
				dataOut.writeFloat(sonarValue);
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send sonar distance data");
			}
		}
		else if(OPCode == SETSTRAIGHT)
		{
			LineFinder finder = new LineFinder(this);
			finder.setStraight();
			sendSucces();
		}
		else if(OPCode == CALIBRATE_COLOR)
		{
			int result = -1;
			if(param1 == 1)
				result = calibrateColor(SensorColour.WHITE);
			if(param1 == 2)
				result = calibrateColor(SensorColour.BLACK);
			if(param1 == 3)
				result = calibrateColor(SensorColour.BROWN);
			try {
				dataOut.writeInt(result);
				dataOut.flush();
			} catch (IOException e) {
				System.out.println("Failed to send sonar calibration data");
			}
		}
		else if (OPCode == PLAY_TUNE)
		{
			if (immediateReturn) {
				PlayTune tune = new PlayTune();
				tune.start();
			} else
				PlayTune.HallOfMountainKing();
		}
	}
	private int whiteCalibration = -1;
	private int blackCalibration = -1;
	private int brownCalibration = -1;
	private int calibrateColor(SensorColour color)
	{
		long startTime = System.currentTimeMillis();
		
		List<Integer> samples = new ArrayList<Integer>();
		
		while(System.currentTimeMillis() < startTime + 5000)
		{
			samples.add(getLightValue());
			try {
				Thread.sleep(100);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		
		long total = 0;
		for(Integer val : samples)
		{
			total += val;
		}
		
		int result = (int) (total / samples.size());
		
		if (color == SensorColour.BLACK)
			blackCalibration = result;
		else if (color == SensorColour.BROWN)
			brownCalibration = result;
		else if (color == SensorColour.WHITE)
			whiteCalibration = result;
		
		if(whiteCalibration != -1 && brownCalibration != -1 && blackCalibration != -1)
		{
			BLACK_BROWN = (blackCalibration + brownCalibration) / 2;
			BROWN_WHITE = (int) (((brownCalibration + whiteCalibration) / 2.0)*1.03);
		}
		
		return result;
	}
	
	private int sonarScan(int direction)
	{
		List<Integer> samples = new ArrayList<Integer>();

		if(direction == -1)
		{
			sonarMotor.rotateTo(-92);
		}
		else if(direction == 1)
		{
			sonarMotor.rotateTo(90);
		}
		else if(direction == 0)
		{
			sonarMotor.rotateTo(0);
		}
		pilot.setAcceleration(13);
		
		samples.add(sonar.getDistance());
		for(int i = 0; i < 2; i++)
		{
			pilot.travel(1);
			samples.add(sonar.getDistance());
		}
		for(int i = 0; i < 2; i++)
		{
			pilot.travel(-1);
			samples.add(sonar.getDistance());
		}

	   	// Sort
		 for (int i=0; i<samples.size()-1; i++) {
		        for (int j=i+1; j<samples.size(); j++) {
		            if (samples.get(i) > samples.get(j)) {
		                //... Exchange elements
		                int temp = samples.get(i);
		                samples.set(i, samples.get(j));
		                samples.set(j, temp);
		            }
		        }
		    }

		 pilot.setAcceleration(20);
		 return samples.get(samples.size()-1);
	}
	
	private int sonarScan()
	{
		List<Integer> samples = new ArrayList<Integer>();
		pilot.setAcceleration(13);
		
		samples.add(sonar.getDistance());
		for(int i = 0; i < 2; i++)
		{
			pilot.travel(1);
			samples.add(sonar.getDistance());
		}
		for(int i = 0; i < 2; i++)
		{
			pilot.travel(-1);
			samples.add(sonar.getDistance());
		}

	   	// Sort
		 for (int i=0; i<samples.size()-1; i++) {
		        for (int j=i+1; j<samples.size(); j++) {
		            if (samples.get(i) > samples.get(j)) {
		                //... Exchange elements
		                int temp = samples.get(i);
		                samples.set(i, samples.get(j));
		                samples.set(j, temp);
		            }
		        }
		    }

		 pilot.setAcceleration(20);
		//return samples.get(samples.size()/2);
		 return samples.get(samples.size()-1);
	}
	public int getSonarDistance()
	{
		return sonar.getDistance();
	}
	
	public int getLightValue()
	{
		return this.light.getNormalizedLightValue();
	}

	public SensorColour getLightColour()
	{
		if(getLightValue() <= BLACK_BROWN)
			return SensorColour.BLACK;
		if(getLightValue() <= BROWN_WHITE)
			return SensorColour.BROWN;
		else
			return SensorColour.WHITE;
	}
	
	public DifferentialPilot getPilot()
	{
		return this.pilot;
	}
	
	public Pose getPose(){
		return poseProvider.getPose();
	}
	public void rotateSonarMotorTo(int angle){
		sonarMotor.rotateTo( angle,false);
	}
}
